﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Collections;

namespace CPF_experement
{
    static class True_Path_Huristic
    {
        static int[][][] allTileAgentHuristics;
        static int size_X;
        static int size_Y;
        static bool[][] grid;
        static bool allowDiagonalMove;

        public static void init(int i_size_X, int i_size_Y, Agent[] allAgents, bool diagonal, bool[][] i_grid)
        {
            size_X = i_size_X;
            size_Y = i_size_Y;
            grid = i_grid;
            allowDiagonalMove=diagonal;
            allTileAgentHuristics = new int[allAgents.Length][][];
            for (int i = 0; i < allAgents.Length; i++)
            {
                int a = allAgents[i].Goal_X;
                int b = allAgents[i].Goal_Y;
                allTileAgentHuristics[i] = setHuristicsForTile(a, b);
            }
        }

        private static int[][] setHuristicsForTile(int X, int Y)
        {
            int a;
            int b;
            int val;
            int[][] ans = new int[size_X][];
            for (int i = 0; i < size_X; i++)
            {
                ans[i] = new int[size_Y];
                for (int j = 0; j < size_Y; j++)
                {
                    ans[i][j] = -1;
                }
            }
            Queue openList = new Queue();
            HashSet<int> closedList = new HashSet<int>();
            openList.Enqueue(X);
            openList.Enqueue(Y);
            openList.Enqueue(0);
            closedList.Add(X * size_X + Y);
            while (openList.Count>0)
            {
                a = (int)openList.Dequeue();
                b = (int)openList.Dequeue();
                val = (int)openList.Dequeue();
                ans[a][b] = val;
                if (isValidTile(a-1,b) && !closedList.Contains((a-1) * size_X + b))
                {
                    insertToOpenList(a-1, b , val + 1, openList);
                    closedList.Add((a-1) * size_X + b);
                }


                if (isValidTile(a,b+1) && !closedList.Contains(a * size_X + b + 1))
                {
                    insertToOpenList(a, b + 1, val + 1, openList);
                    closedList.Add(a * size_X + b + 1);
                }

                if (isValidTile(a+1,b) && !closedList.Contains((a + 1) * size_X + b))
                {
                    insertToOpenList(a + 1, b , val + 1, openList);
                     closedList.Add((a+1) * size_X + b);
                }

                if (isValidTile(a,b-1) && !closedList.Contains(a * size_X + b - 1))
                {
                    insertToOpenList(a , b-1, val + 1, openList);
                    closedList.Add(a * size_X + b-1);
                }

                if (allowDiagonalMove)
                {
                    if (isValidTile(a-1,b+1) && !closedList.Contains((a - 1) * size_X + b + 1))
                    {
                        insertToOpenList(a - 1, b + 1, val + 1, openList);
                        closedList.Add((a-1) * size_X + b+1);
                   }

                    if (isValidTile(a+1,b+1) && !closedList.Contains((a + 1) * size_X + b + 1))
                    {
                        insertToOpenList(a + 1, b + 1, val + 1, openList);
                        closedList.Add((a+1) * size_X + b+1);
                      }

                    if (isValidTile(a+1,b-1) && !closedList.Contains((a + 1) * size_X + b - 1))
                    {
                        insertToOpenList(a + 1, b - 1, val + 1, openList);
                        closedList.Add((a + 1) * size_X + b - 1);
                    }

                    if (isValidTile(a-1,b-1) && !closedList.Contains((a - 1) * size_X + b - 1))
                    {
                        insertToOpenList(a - 1, b - 1, val + 1, openList);
                        closedList.Add((a - 1) * size_X + b - 1);
                    }
                }
            }
            return ans;
        }

        private static void insertToOpenList(int X, int Y, int val, Queue OL)
        {
            OL.Enqueue(X);
            OL.Enqueue(Y);
            OL.Enqueue(val);
        }
        public static int getHuristic(int agent, int X, int Y)
        {
            return allTileAgentHuristics[agent][X][Y];
        }
        static public bool isValidTile(int X, int Y)
        {
            if (X < 0 || X >= size_X || Y < 0 || Y >= size_Y)
                return false;
            return !grid[X][Y];
        }
        public static int getGrisSize()
        {
            return size_X;
        }
        public static LinkedList<MDDNode> getNextLevelForTile(int pos_X, int pos_Y, int agentNum, int maxCost)
        {
            LinkedList<MDDNode> ans = new LinkedList<MDDNode>();
            if (getHuristic(agentNum, pos_X, pos_Y) <= maxCost)
            {
                ans.AddLast(new MDDNode(pos_X, pos_Y));
            }
            if (isValidTile(pos_X - 1, pos_Y) && getHuristic(agentNum, pos_X - 1, pos_Y) <= maxCost)
            {
                 ans.AddLast(new MDDNode(pos_X-1, pos_Y));
            }
            if (isValidTile(pos_X , pos_Y + 1) && getHuristic(agentNum, pos_X , pos_Y + 1) <= maxCost)
            {
                ans.AddLast(new MDDNode(pos_X , pos_Y + 1));
            }
            if (isValidTile(pos_X + 1, pos_Y) && getHuristic(agentNum, pos_X + 1, pos_Y) <= maxCost)
            {
                ans.AddLast(new MDDNode(pos_X + 1, pos_Y));
            }
            if (isValidTile(pos_X , pos_Y - 1) && getHuristic(agentNum, pos_X , pos_Y - 1) <= maxCost)
            {
                ans.AddLast(new MDDNode(pos_X , pos_Y - 1));
            }
            return ans;
        }
        
    }
}
